#include "canopen.hpp"

using namespace CANalyst2;

int CANOPEN::do_write(int ch, uint8_t slave, uint16_t index, uint8_t subindex)
{
    std::cout << "SDO frame send: " << std::hex << index << ":" << (uint32_t)subindex << std::endl;
    if (ch == 0) {
        auto send_frame = frames0;
        for (int i = 0; i < 3; ++i)
        {
            int res = can->send(send_frame, 100, 0);
            usleep(20 * 1000);

            int timeout = 50;
            int size;
            do
            {
                size = can->recv(frames0, 1, 0);
                std::cout << "SDO recv frame size:" << frames0.size() << std::endl;
                // std::cout << "recv frame count: " << size << std::endl;
                for (int i = 0; i < frames0.size(); ++i)
                {
                    uint16_t _cmd = frames0[i].Data[0];
                    uint16_t _index = frames0[i].Data[1] + frames0[i].Data[2] * 0x0100;
                    uint8_t _subidnex = frames0[i].Data[3];
                    uint32_t _error;
                    memcpy(&_error, frames0[i].Data + 4, 4);
                    if (frames0[i].ID == 0x580 + slave && _index == index && _subidnex == subindex)
                    {
                        if (_cmd == 0x60)
                        {
                            std::cout << std::hex << index << ":" << subindex << "write OK" << std::endl;
                            return 0;
                        }
                        else if (_cmd == 0x80)
                        {
                            std::cout << std::hex << index << ":" << subindex << "write fail: " << _error << std::endl;
                            raise(SIGABRT);
                            return _error;
                        }
                        else if (_cmd == 0x4F || _cmd == 0x4B || _cmd == 0x43) 
                        {
                            parse_recv_sdo(index, subindex, &frames0[i], _cmd);
                            return 0;
                        }
                        else
                        {
                            std::cout << std::hex << index << ":" << subindex << "datagram error, " << _cmd << std::endl;
                            raise(SIGABRT);
                            return -1;
                        }
                    }
                }
            } while (size > 0 && (--timeout) > 0);
        }
        std::cout << "CAN not respond" << std::endl;
        raise(SIGABRT);
        return -2;
    }
    else if (ch == 1) {
        auto send_frame = frames1;
        for (int i = 0; i < 3; ++i)
        {
            int res = can->send(send_frame, 100, 1);
            usleep(20 * 1000);

            int timeout = 50;
            int size;
            do
            {
                size = can->recv(frames1, 1, 1);
                std::cout << "SDO recv frame size:" << frames1.size() << std::endl;
                // std::cout << "recv frame count: " << size << std::endl;
                for (int i = 0; i < frames1.size(); ++i)
                {
                    uint16_t _cmd = frames1[i].Data[0];
                    uint16_t _index = frames1[i].Data[1] + frames1[i].Data[2] * 0x0100;
                    uint8_t _subidnex = frames1[i].Data[3];
                    uint32_t _error;
                    memcpy(&_error, frames1[i].Data + 4, 4);
                    if (frames1[i].ID == 0x580 + slave && _index == index && _subidnex == subindex)
                    {
                        if (_cmd == 0x60)
                        {
                            std::cout << std::hex << index << ":" << subindex << "write OK" << std::endl;
                            return 0;
                        }
                        else if (_cmd == 0x80)
                        {
                            std::cout << std::hex << index << ":" << subindex << "write fail: " << _error << std::endl;
                            raise(SIGABRT);
                            return _error;
                        }
                        else if (_cmd == 0x4F || _cmd == 0x4B || _cmd == 0x43) 
                        {
                            parse_recv_sdo(index, subindex, &frames1[i], _cmd);
                            return 0;
                        }
                        else
                        {
                            std::cout << std::hex << index << ":" << subindex << "datagram error, " << _cmd << std::endl;
                            raise(SIGABRT);
                            return -1;
                        }
                    }
                }
            } while (size > 0 && (--timeout) > 0);
        }
        std::cout << "CAN not respond" << std::endl;
        raise(SIGABRT);
        return -2;
    }
}

void CANOPEN::parse_recv_sdo(uint16_t index, uint8_t subindex, CanStamped *p_recv_frame, uint16_t cmd)
{   
    std::cout << "Parse receive SDO frame" << std::endl;
    std::cout << std::hex << "index:0x" << index << " subindex:" << subindex << std::endl;
    // std::cout << "p_recv_frame->Data[4]:" << std::hex << static_cast<int>(p_recv_frame->Data[4])
    //       << " Data[5]:" << static_cast<int>(p_recv_frame->Data[5])
    //       << " Data[6]:" << static_cast<int>(p_recv_frame->Data[6])
    //       << " Data[7]:" << static_cast<int>(p_recv_frame->Data[7]) << std::endl;
    switch(cmd)
    {
        case 0x4F:
        {
            auto recv_raw_data = static_cast<uint8_t> (p_recv_frame->Data[4]);
            if (index == VALID_WORK_MODE_INDEX) {
                _valid_work_mode = recv_raw_data;
                std::cout << "Valid work mode is:" << static_cast<int>(_valid_work_mode) << std::endl;
            }
            else if (index == SERVO_WORK_MODE_INDEX) {
                _servo_work_mode = recv_raw_data;
                std::cout << "Servo work mode is:" << static_cast<int>(_servo_work_mode) << std::endl;
            }
            else {
                _is_parsed_ok = false;
                return;
            }
            _is_parsed_ok = true;
            return;
        }
        break;
        case 0x4B:
        {
            auto recv_raw_data = static_cast<uint16_t> (p_recv_frame->Data[4] | (p_recv_frame->Data[5] << 8));
            if (index == SERVO_CTRL_WORD_INDEX) {
                _current_ctrl_word = recv_raw_data;
                std::cout << "Current control word is:" << _current_ctrl_word << std::endl;
            }
            else if (index == SERVO_STATUS_WORD_INDEX) {
                _current_status_word = recv_raw_data;
                std::cout << "Current status word is:" << _current_status_word << std::endl;
            }
            else if (index == ACTUAL_CURRENT_INDEX) {
                _actual_current = static_cast<int16_t> (recv_raw_data);
                std::cout << "Actual current is:" << _actual_current << std::endl;
            }
            else if (index == STOP_OPTION_INDEX) {
                _stop_option = static_cast<uint16_t> (recv_raw_data);
                std::cout << "Stop option is:" << _stop_option << std::endl;
            }
            else 
            {
                _is_parsed_ok = false;
                return;
            }
            _is_parsed_ok = true;
            return;
        }
        break;
        case 0x43:
        {
            auto recv_raw_data = static_cast<int32_t> (p_recv_frame->Data[4] | (p_recv_frame->Data[5] << 8) | 
                                    (p_recv_frame->Data[6] << 16) | (p_recv_frame->Data[7] << 24));
            if (index == TARGET_POS_INDEX) {
                _target_pos = recv_raw_data;
                std::cout << "Target position is:" << _target_pos << std::endl;
            }
            else if (index == ACTUAL_POS_INDEX || index == ACTUAL_POS_INDEX_ZEROERR) {
                _actual_pos = recv_raw_data;
                std::cout << "Actual position is:" << _actual_pos << std::endl;
            }
            else if (index == TARGET_VELOCITY_INDEX) {
                _target_velocity = recv_raw_data;
                std::cout << "Target velocity is:" << _target_velocity << std::endl;
            }
            else if (index == ACTUAL_VELOCITY_INDEX) {
                _actual_velocity = recv_raw_data;
                std::cout << "Actual velocity is:" << _actual_velocity << std::endl;
            }
            else if (index == PROFILED_VELOCITY_INDEX) {
                _profiled_vel = static_cast<uint32_t>(recv_raw_data);
                std::cout << "Profiled velocity is:" << _profiled_vel << std::endl;
            }
            else if (index == PROFILED_ACC_INDEX) {
                _profiled_acc = static_cast<uint32_t>(recv_raw_data);
                std::cout << "Profiled acceleration is:" << _profiled_acc << std::endl;
            }
            else if (index == PROFILED_DEC_INDEX) {
                _profiled_dec = static_cast<uint32_t>(recv_raw_data);
                std::cout << "Profiled deceleration is:" << _profiled_dec << std::endl;
            }
            else {
                _is_parsed_ok = false;
                return;
            }
            _is_parsed_ok = true;
            return;
        }    
        break;
        default:
            _is_parsed_ok = false;
            return;
        break;
    }
}